/**
 * @author Yunfeng Bai
 *
 * 2010-11-14
 */
package simulation;

import java.util.ArrayList;
import java.util.Collections;
import java.util.Comparator;
import java.util.List;
import java.util.Map;
import java.util.TreeMap;

import rigidbody.RigidBody;
import util.Util;
import collision.Collision;
import collision.OBB;

public class CollisionDetector {
	private class Interval
	{
		public double l, r;
		public OBB obb;
		public RigidBody body;
		
		public Interval(double l, double r, RigidBody body, OBB obb)
		{
			this.l = l;
			this.r = r;
			this.body = body;
			this.obb = obb;
		}
	}
	
	private class IntervalComparator implements Comparator<Interval>
	{
		@Override
		public int compare(Interval i1, Interval i2)
		{
			if (i1.l < i2.l) return -1;
			if (i1.l > i2.l) return 1;
			// i1.l == i2.l;
			if (i1.r < i2.r) return -1;
			if (i1.r > i2.r) return 1;
			return 0;
		}
	}
	
	private Interval mapRigidBodyToInterval(RigidBody body, int dim)
	{
		OBB obb = body.getOBBTree().getRootOBB();
		double min = Util.getA(obb.center, dim);
		double max = min;
		for (int i = 0; i < 3; i++)
		{
			double dimExtent = Util.getA(obb.axes[i], dim);
			dimExtent *= obb.size[dim];
			min -= Math.abs(dimExtent);
			max += Math.abs(dimExtent);
		}
		return new Interval(min, max, body, obb);
	}
	
	TreeMap<Collision, Integer> collisionCandidates =
			new TreeMap<Collision, Integer>(new CollisionComparator());
	
	private class CollisionComparator implements Comparator<Collision>
	{
		@Override
		public int compare(Collision o1, Collision o2)
		{
			if (o1.object1.getId() < o2.object1.getId()) return -1;
			if (o1.object1.getId() > o2.object1.getId()) return 1;
			if (o1.object2.getId() < o2.object2.getId()) return -1;
			if (o1.object2.getId() > o2.object2.getId()) return 1;
			return 0;
		}
	}
	
	private boolean hasIntersection(Interval i1, Interval i2)
	{
		return (i2.l <= i1.l && i1.l <= i2.r) || (i1.l <= i2.l && i2.l <= i1.r);
	}
	
	private boolean OBBColliding(RigidBody b1, RigidBody b2)
	{
		return false;
	}
	
	public List<Collision> detect(List<RigidBody> bodies)
	{
		List<Collision> broadPhaseCollisions = broadPhase(bodies);
		for (Collision c : broadPhaseCollisions)
		{
			
		}
		return null;
	}

	private List<Collision> broadPhase(List<RigidBody> bodies) {
		IntervalComparator comparator = new IntervalComparator();
		
		collisionCandidates.clear();
		
		// Do sweep and prune on X, Y, Z axes.
		List<Interval> intervals = new ArrayList<Interval>();
		List<Interval> activeList = new ArrayList<Interval>();
		List<Integer> removeList = new ArrayList<Integer>();
		for (int dim = 0; dim < 3; dim++)
		{
			// Map rigid bodies to one axis.
			intervals.clear();
			activeList.clear();
			for (int i = 0; i < bodies.size(); i++)
			{
				intervals.add(mapRigidBodyToInterval(bodies.get(i), dim));
			}
			
			Collections.sort(intervals, comparator);
			
			for (int i = 0; i < bodies.size(); i++)
			{
				Interval interval = intervals.get(i);
				int activeListCount = activeList.size();

				removeList.clear();
				for (int j = 0; j < activeListCount; j++)
				{
					Interval activeInterval = activeList.get(j);
					if (activeInterval.r <= interval.l)
					{
						removeList.add(j);
						continue;
					}
					
					if (hasIntersection(interval, activeInterval))
					{
						if (interval.obb.isColliding(activeInterval.obb))
						{
							Collision c = GetCollision(interval, activeInterval);
							Integer count = collisionCandidates.get(c);
							if (count != null)
								collisionCandidates.put(c, count + 1);
							else
								collisionCandidates.put(c, 1);
						}
					}
				}
				
				for (int j = removeList.size() - 1; j >= 0; j--)
					activeList.remove(removeList.get(j));
			}
		}
		
		List<Collision> broadPhaseCollisions = new ArrayList<Collision>();
		for (Map.Entry<Collision, Integer> e : collisionCandidates.entrySet())
		{
			if (e.getValue() == 3)
				broadPhaseCollisions.add(e.getKey());
		}
		
		return broadPhaseCollisions;
	}
	
	private Collision GetCollision(Interval i1, Interval i2)
	{
		Collision c = new Collision();
		if (i1.body.getId() < i2.body.getId())
		{
			c.object1 = i1.body;
			c.object2 = i2.body;
		}
		else
		{
			c.object1 = i2.body;
			c.object2 = i1.body;
		}
		return c;
	}
}
